/***************************************************************************
 *   Copyright (C) 2010 by Fabio Galbusera   *
 *   fabio.galbusera@uni-ulm.de   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "CMatrix.h"

CMatrix::CMatrix(int r, int c)
{
	nr=r;
	nc=c;
	values=(double *) calloc(r*c,sizeof(double));
	setZero();
}


CMatrix::~CMatrix()
{
	free(values);
}

void CMatrix::setIdentity()
{
	int i,j;
	for (i=0;i<nr;i++)
		for (j=0;j<nc;j++)
		{
			if (i==j)
				set(i,j,1.);
			else
				set(i,j,0.);
		}
}

void CMatrix::set(int r, int c, double v)
{
	values[r*nc+c]=v;
}

void CMatrix::setP1(int r, int c, double v)
{
	values[(r-1)*nc+(c-1)]=v;
}

double CMatrix::get(int r, int c)
{
	return values[r*nc+c];
}

double CMatrix::getP1(int r, int c)
{
	return values[(r-1)*nc+(c-1)];
}

void CMatrix::print()
{
	FILE *out;
	out=fopen("matrix.txt","w");
	int i,j;
	for (i=0;i<nr;i++)
	{
	    fprintf(out,"LINE %d ->",i);
		for (j=0;j<nc;j++)
		{
			fprintf(out,"%f  ",get(i,j));
		}
		fprintf(out,"\n");
	}
	fclose(out);
}

void CMatrix::swapRows(int r1, int r2)
{
	int i;
	double *temp=(double *) calloc(nc,sizeof(double));
	for (i=0;i<nc;i++)
		temp[i]=get(r1,i);
	for (i=0;i<nc;i++)
		set(r1,i,get(r2,i));
	for (i=0;i<nc;i++)
		set(r2,i,temp[i]);
	free (temp);
}

void CMatrix::setZero()
{
	int i,j;
	for (i=0;i<nr;i++)
	{
		for (j=0;j<nc;j++)
		{
			set(i,j,0.);
		}
	}
}

CMatrix* CMatrix::solveGauss(CMatrix *rhs)
{

    double tmp,pvt,*t;
    int i,j,k,itmp;

    int n=nr;

    CMatrix *x=new CMatrix(n,1);
    x->setZero();

    for (i=0;i<n;i++)
    {             // outer loop on rows
        pvt = get(i,i);              // get pivot value
        if (fabs(pvt) < EPS)
	{
            for (j=i+1;j<n;j++)
	    {
                if(fabs(pvt = get(j,i)) >= EPS) break;
            }
            if (fabs(pvt) < EPS) return NULL;     // nowhere to run!
	    swapRows(i,j);
	    rhs->swapRows(i,j);
        }
	// (virtual) Gaussian elimination of column
        for (k=i+1;k<n;k++)
	{       // alt: for (k=n-1;k>i;k--)
            tmp = get(k,i)/pvt;double suma,m;
            for (j=i+1;j<n;j++)
	    {   // alt: for (j=n-1;j>i;j--)
                set(k,j,get(k,j)-tmp*get(i,j));
            }
            rhs->set(k,0,rhs->get(k,0)-tmp*rhs->get(i,0));
        }
     }
	// Do back substitution
     for (i=n-1;i>=0;i--)
     {
		x->set(i,0,rhs->get(i,0));
		for (j=n-1;j>i;j--)
		{
			x->set(i,0,x->get(i,0)-get(i,j)*x->get(j,0));
		}
		x->set(i,0,x->get(i,0)/get(i,i));
     }
     return x;
}

void CMatrix::add(int r, int c, double v)
{
	set(r,c,get(r,c)+v);
}

CMatrix* CMatrix::solveGaussSeidel(CMatrix *rhs)
{
	double suma,m;
	double error=1.;
	double start=0.;
	int n=nr;

    	CMatrix *x=new CMatrix(n,1);
    	x->setZero();
	CMatrix *xold=new CMatrix(n,1);
    	xold->setZero();

	for(int i=0;i<n;++i)
	{
                xold->set(i,0,start);
                x->set(i,0,start);
        }
	for(int it=1;it<ITMAX;++it)
	{
		printf("iteration %d, error %f\n",it,error);
		if(error>EPS)
		{
			for(int i=0;i<n;++i)
			{
				suma=0;
				for(int j=0;j<i;++j)
				{
					suma+=get(i,j)*x->get(j,0);
				}
				for(int j=i+1;j<n;++j)
				{
					suma+=get(i,j)*xold->get(j,0);
				}
				x->set(i,0,(rhs->get(i,0)-suma)/get(i,i));
			}
		}
		else break;
		error=0.;
		for(int k=0;k<n;++k)
		{
			m=fabs(x->get(k,0)-xold->get(k,0));
			if(error<m)
				error=m;
			xold->set(k,0,x->get(k,0));
		}
	}
	delete xold;
	return x;
}

CMatrix* CMatrix::copy()
{
	int i,j;
	CMatrix *newMatrix=new CMatrix(nr, nc);
	for (i=0;i<nr;i++)
		for (j=0;j<nc;j++)
		{
			newMatrix->set(i,j,get(i,j));
		}
	return newMatrix;
}

int CMatrix::countNoZeros()
{
	int i,j;
	int nozeros=0;
	for (i=0;i<nr;i++)
		for (j=0;j<nc;j++)
		{
			if (get(i,j)!=0.)
				nozeros++;
		}
	return nozeros;
}

int CMatrix::getNRows()
{
	return nr;
}

int CMatrix::getNCols()
{
	return nc;
}


